/******************************************************************************
Class:Quaternion
Implements:
Author:Rich Davison
Description:VERY simple Quaternion class. Students are encouraged to modify 
this as necessary...if they dare.

Quaternions aren't really discussed much in the graphics module, but the 
MD5Mesh class uses them internally, as MD5 files store their rotations
as quaternions.

I hate Quaternions.

-_-_-_-_-_-_-_,------,   
_-_-_-_-_-_-_-|   /\_/\   NYANYANYAN
-_-_-_-_-_-_-~|__( ^ .^) /
_-_-_-_-_-_-_-""  ""   

*//////////////////////////////////////////////////////////////////////////////

#pragma once

#include <iostream>
#include "common.h"
#include "Matrix4.h"

#define M_PI	3.141592654

class Matrix4;

class Quaternion	{
public:
	Quaternion(void);
	Quaternion(float x, float y, float z, float w);

	~Quaternion(void);

	float x;
	float y;
	float z;
	float w;

	void	Normalise();
	Matrix4 ToMatrix() const;


	Quaternion	Conjugate() const;
	void		GenerateW();	//builds 4th component when loading in shortened, 3 component quaternions

	static Quaternion EulerAnglesToQuaternion(float pitch, float yaw, float roll);
	static Quaternion AxisAngleToQuaterion(const Vector3& vector, float degrees);
		//***convert to degree****///
	static Vector3 QuaternionToEulerAngles(const Quaternion& QOrientation);

	static void RotatePointByQuaternion(const Quaternion &q, Vector3 &point);

	static Quaternion FromMatrix(const Matrix4 &m);

	static float Dot(const Quaternion &a, const Quaternion &b);

	Quaternion operator *(const Quaternion &a) const;
	Quaternion operator *(const Vector3 &a) const;

	inline friend std::ostream& operator<<(std::ostream& o, const Quaternion& q){
		o << "Quat(" << q.x << "," << q.y << "," << q.z <<  "," << q.w << ")" << std::endl;
		return o;
	}

	inline Quaternion operator+(const Quaternion &q) {
		return Quaternion(x+q.x, y+q.y, z+q.z, w+q.w);
	}

	double getRoll(){
		double rotateYa0 = -2.0*(x*y - w*z);
        double rotateY = 0.0;
        if( rotateYa0 >= 1.0 )
            rotateY = M_PI/2.0;
        else if( rotateYa0 <= -1.0 )
            rotateY = -M_PI/2.0;
        else rotateY = asin(rotateYa0);

		return rotateY;
	}

	//These two don't work??
	double getPitch(){
		double rotateXa0 = 2.0*(y*z + w*x);
        double rotateXa1 = w*w - x*x - y*y + z*z;
        double rotateX = 0.0;
        if (rotateXa0 != 0.0 && rotateXa1 != 0.0) 
            rotateX = atan2(rotateXa0, rotateXa1);

		return rotateX;
	}

	double getYaw(){
		double rotateZa0 = 2.0*(x*y + w*z);
        double rotateZa1 = w*w + x*x - y*y - z*z;
        double rotateZ = 0.0;
        if (rotateZa0 != 0.0 && rotateZa1 != 0.0)
            rotateZ = atan2(rotateZa0, rotateZa1);

		return rotateZ;
	}
};